% Entry Trajectory Optimization by Second-Order Cone Programming
% 通过在无控的状态下对运动方程进行积分，选取航迹倾角由负值翻转到正值的飞行高度作为转折点
clc
clear
close all;
m = 104305;
S = 391.22;
deg2rad = pi/180;
R0 = 6378137;
g0 = 9.81;
Vc = sqrt(R0*g0);
rho0 = 1.2256;
hs = 7254.24;
N = 200;

rad0 = (1e5 + R0)/R0;
radf = (25e3 + R0)/R0;
V0 = 7450/Vc;
Vf = 625/Vc;
lon0 = 0;
lat0 = 0;
% fpa0 = -0.5*deg2rad;
fpa0 = -1*deg2rad;
azi0 = 0;
init = [rad0; lon0; lat0; fpa0; azi0];
statep = init;
e0 = 1/rad0 - V0^2/2;
ef = 1/radf - Vf^2/2;
e = linspace(e0, ef, N+1);
de = (ef - e0)/N;
res = nan(6, N+1);
res(1,:) = e;

for i = 1:N
    res(2:6,i) = statep;
    state = rk4(statep, e(i+1), de);
    statep = state;
    if(state(4) > 0)
        break;
    end
end
rad00 = linspace(rad0, radf, N+1);
plot(res(1,1:i),res(5,1:i));
plot(res(1,1:i),res(2,1:i));